#include "stm32f1xx_hal.h"
#include "Control.h"
#include "oled.h"
#include "main.h"


// /*
// 设置右前轮的变量
// */
// float RF_Kp=1;
// float RF_Ki=1;
// float RF_Kd=0;
// int16_t RF_PWMMax;
// int16_t RF_targetmaichong,RF_actualmaichong,RF_error,RF_Last_error,\
// RF_Last_Last_error,RF_d_error,RF_dd_error,RF_PWM;

float RF_target_speed = 0; // 各个轮子速度变量
float LF_target_speed = 0;
float RB_target_speed = 0;
float LB_target_speed = 0;

PID LF_PID = { // 定义左前电机(2号电机)PID控制结构体
    .P = 3.7,
    .I = 2.7,
    .D = 0.0,
    .Target_maichong = 0,
    .Actual_maichong = 0,
    .Error0 = 0,
    .Error1 = 0,
    .Error2 = 0,
    .Output = 0};
PID LB_PID = { // 定义左后电机（4号电机）PID控制结构体
    .P = 3.7,
    .I = 2.7,
    .D = 0.0,
    .Target_maichong = 0,
    .Actual_maichong = 0,
    .Error0 = 0,
    .Error1 = 0,
    .Error2 = 0,
    .Output = 0};

PID RF_PID = { // 定义右前电机（1号电机）PID控制结构体
    .P = 3.7,
    .I = 2.7,
    .D = 0.0,
    .Target_maichong = 0,
    .Actual_maichong = 0,
    .Error0 = 0,
    .Error1 = 0,
    .Error2 = 0,
    .Output = 0};
PID RB_PID = { // 定义右后电机（3号电机）PID控制结构体
    .P = 3.7,
    .I = 2.7,
    .D = 0.0,
    .Target_maichong = 0,
    .Actual_maichong = 0,
    .Error0 = 0,
    .Error1 = 0,
    .Error2 = 0,
    .Output = 0};



/*
@brief:控制电机1的方向
@note：无
@param：mode：-1：反转；0：停止；1：正转
@retval None
*/
void Motor1_forward(int8_t mode)
{
    if (mode < 0)
    {
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_0, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_1, GPIO_PIN_SET);
    }
    else if (mode == 0)
    {
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_0, GPIO_PIN_SET);
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_1, GPIO_PIN_SET);
    }
    else
    {
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_0, GPIO_PIN_SET);
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_1, GPIO_PIN_RESET);


    }
}

/*
@brief:控制电机2的方向
@note：无
@param：mode：-1：反转；0：停止；1：正转
@retval None
*/
void Motor2_forward(int8_t mode)
{
    if (mode < 0)
    {
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_2, GPIO_PIN_SET);
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_3, GPIO_PIN_RESET);
    }
    else if (mode == 0)
    {
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_2, GPIO_PIN_SET);
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_3, GPIO_PIN_SET);
    }
    else
    {
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_2, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_3, GPIO_PIN_SET);

    }
}

/*
@brief:控制电机3的方向
@note：无
@param：mode：-1：反转；0：停止；1：正转
@retval None
*/
void Motor3_forward(int8_t mode)
{
    if (mode < 0)
    {
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_4, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_5, GPIO_PIN_SET);
    }
    else if (mode == 0)
    {
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_4, GPIO_PIN_SET);
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_5, GPIO_PIN_SET);
    }
    else
    {
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_4, GPIO_PIN_SET);
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_5, GPIO_PIN_RESET);
    }
}

/*
@brief:控制电机4的方向
@note：无
@param：mode：-1：反转；0：停止；1：正转
@retval None
*/
void Motor4_forward(int8_t mode)
{
    if (mode < 0)
    {
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_8, GPIO_PIN_SET);
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_9, GPIO_PIN_RESET);
    }
    else if (mode == 0)
    {
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_8, GPIO_PIN_SET);
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_9, GPIO_PIN_SET);
    }
    else
    {
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_8, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_9, GPIO_PIN_SET);
    }
}

/*
@brief:控制左侧电机的方向
@note：无
@param：mode：-1：反转；0：停止；1：正转
@retval None
*/
void left_motor_forward(int8_t mode)
{
    Motor2_forward(mode);
    Motor4_forward(mode);
}

/*
@brief:控制右侧电机的方向
@note：无
@param：mode：-1：反转；0：停止；1：正转
@retval None
*/
void right_motor_forward(int8_t mode)
{
    Motor1_forward(mode);
    Motor3_forward(mode);
}

/*
@brief:计算pid的输出值
@note：无
@param：对应的pid控制结构体、目标速度
@retval 输出值
*/
int16_t PID_Calculate(PID *pid, float target_speed)
{
    pid->Target_maichong = (int16_t)(target_speed * maichong * tim6_Time / C); // 目标值
    pid->Error0 = pid->Target_maichong - pid->Actual_maichong; // 误差
    pid->change_output = (int16_t)(pid->P * (pid->Error0-pid->Error1) + pid->I * pid->Error0 + pid->D * (pid->Error0 - 2 * pid->Error1 + pid->Error2)); // 增加值
    pid->Error2 = pid->Error1;
    pid->Error1 = pid->Error0;
    return pid->change_output;
}
/*
@brief:设置目标电机的速度
@note：无
@param：电机编号,1,2,3,4
@param: 目标速度,最大速度0.62m/s
@retval None
*/
void Motor_Set_Speed(uint8_t motor_num, float *target_speed)
{
    switch (motor_num)
    {
    case 1:
        if(*target_speed>-0.02&&*target_speed<0.02)
        {
            *target_speed=0;
            RF_PID.Output=0;
            Motor1_forward(0);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0); // 电机1的PWM
        }
        else if(*target_speed >=0.02)
        {
            RF_PID.P = 2.0;
            RF_PID.I = 3.0;
            RF_PID.D = 0.0;
            RF_PID.Output+=PID_Calculate(&RF_PID, *target_speed);
            if(RF_PID.Output<0)RF_PID.Output=0;
            else if(RF_PID.Output>7200)RF_PID.Output=7200;
            Motor1_forward(1);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, RF_PID.Output); // 电机1的PWM
        }
        else
        {
            RF_PID.P = 3.7;
            RF_PID.I = 2.7;
            RF_PID.D = 0.0;
            RF_PID.Output-=PID_Calculate(&RF_PID, *target_speed);
            if(RF_PID.Output<0)RF_PID.Output=0;
            else if(RF_PID.Output>7200)RF_PID.Output=7200;
            Motor1_forward(-1);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, RF_PID.Output);
        }
        break;
    case 2:
        if(*target_speed>-0.02&&*target_speed<0.02)
        {
            *target_speed=0;
            LF_PID.Output=0;
            Motor2_forward(0);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0); // 电机1的PWM
        }
        else if(*target_speed >=0.02)
        {
            LF_PID.P = 3.0;
            LF_PID.I = 3.0;
            LF_PID.D = 0.0;
            LF_PID.Output+=PID_Calculate(&LF_PID, *target_speed);
            if(LF_PID.Output<0)LF_PID.Output=0;
            else if(LF_PID.Output>7200)LF_PID.Output=7200;
            Motor2_forward(1);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, LF_PID.Output); // 电机1的PWM
        }
        else
        {
            LF_PID.P = 2.5;
            LF_PID.I = 1.6;
            LF_PID.D = 0.0;
            LF_PID.Output-=PID_Calculate(&LF_PID, *target_speed);
            if(LF_PID.Output<0)LF_PID.Output=0;
            else if(LF_PID.Output>7200)LF_PID.Output=7200;
            Motor2_forward(-1);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, LF_PID.Output);
        }
        break;
    case 3:
        if(*target_speed>-0.02&&*target_speed<0.02)
        {
            *target_speed=0;
            RB_PID.Output=0;
            Motor3_forward(0);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 0); // 电机1的PWM
        }
        else if(*target_speed >=0.02)
        {
            RB_PID.P = 2.0;
            RB_PID.I = 1.6;
            RB_PID.D = 1.5;
            RB_PID.Output+=PID_Calculate(&RB_PID, *target_speed);
            if(RB_PID.Output<0)RB_PID.Output=0;
            else if(RB_PID.Output>7200)RB_PID.Output=7200;
            Motor3_forward(1);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, RB_PID.Output); // 电机1的PWM
        }
        else
        {
            RB_PID.P = 2.0;
            RB_PID.I = 3.0;
            RB_PID.D = 0.0;
            RB_PID.Output-=PID_Calculate(&RB_PID, *target_speed);
            if(RB_PID.Output<0)RB_PID.Output=0;
            else if(RB_PID.Output>7200)RB_PID.Output=7200;
            Motor3_forward(-1);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, RB_PID.Output);
        }
        break;
    case 4:
        if(*target_speed>-0.02&&*target_speed<0.02)
        {
            *target_speed=0;
            LB_PID.Output=0;
            Motor4_forward(0);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 0); // 电机1的PWM
        }
        else if(*target_speed >=0.02)
        {
            LB_PID.P = 3.0;
            LB_PID.I = 2.0;
            LB_PID.D = 0.0;
            LB_PID.Output+=PID_Calculate(&LB_PID, *target_speed);
            if(LB_PID.Output<0)LB_PID.Output=0;
            else if(LB_PID.Output>7200)LB_PID.Output=7200;
            Motor4_forward(1);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, LB_PID.Output); // 电机1的PWM
        }
        else
        {
            LB_PID.P = 1.8;
            LB_PID.I = 1.8;
            LB_PID.D = 0.0;
            LB_PID.Output-=PID_Calculate(&LB_PID, *target_speed);
            if(LB_PID.Output<0)LB_PID.Output=0;
            else if(LB_PID.Output>7200)LB_PID.Output=7200;
            Motor4_forward(-1);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, LB_PID.Output);
        }
        break;
    default:
        break;
    }
}
